#include <FBFlow.h>
#include <Opencv.h>
#include <matching.h>
#include <compareFeatures.h>
#include <SPRH2d.h>
#include <plot.h>
#include <combineImage.h>
#include <stateManipulator.h>
//#include <KLTFlow.h>

#define MODE 1 //0:save mode 1:comparemode

#define JISSEN
// #define RESEARCH

int main (int argc, char **argv)
{
  cv::Mat src_img;

  // string dirname("0918/dgc/33");
  //string dirname("1106/dgc/15");
  string dirname("project-dsc/0128/origin");
  // compareCloth *cc;
  // compareCloth *cs;
  compareCloth cc;
  compareCloth cs;
  // compareCloth cc("data", 5, 11);
  // compareCloth cs("step", 1, 5);
  // cc = &compareCloth("data", 5, 11);
  // cs = &compareCloth("step", 1, 5);

  int image_count = 1;
  int end_count = -1;

  int save_count = 1;


#ifdef RESEARCH
  //to use reserch
  cc.init("data", 5, 11);
  cs.init("step", 1, 5);
  vector<float> ccThre, csThre;
  //set cc threshold
  // ccThre.push_back(0.35);
  // ccThre.push_back(0.45);
  // ccThre.push_back(0.35);
  ccThre.push_back(0.36);
  ccThre.push_back(0.47);
  ccThre.push_back(0.38);
  cc.setThreVec(ccThre);
  //set cs threshold
  csThre.push_back(0.21);
  csThre.push_back(0.35);
  csThre.push_back(0.28);
  cs.setThreVec(csThre);
  vector<int> stateVec;
  stateVec.push_back(1);
  stateVec.push_back(2);
  stateVec.push_back(3);
  stateVec.push_back(4);
  stateVec.push_back(6);
  stateVec.push_back(7);
  stateVec.push_back(10);
  stateVec.push_back(11);
  stateVec.push_back(12);
  stateVec.push_back(13);
  stateVec.push_back(14);
  cc.sm.setAliasVector(stateVec);
  cc.sm.setProbability(1, 2, 1.0);
  cc.sm.setProbability(2, 3, 1.0);
  cc.sm.setProbability(3, 4, 1.0);
  cc.sm.setProbability(1, 6, 0.1);
  cc.sm.setProbability(1, 7, 0.1);
  //cc.sm.setProbability(1, 7, 1.0);
  cc.sm.setProbability(1, 10, 0.1);
  cc.sm.setProbability(1, 11, 0.5);
  cc.sm.setProbability(1, 12, 0.5);
  cc.sm.setProbability(1, 13, 0.3);
  //cc.sm.setProbability(1, 13, 0.0);
  cc.sm.setProbability(1, 14, 0.5);
  //cc.sm.setProbability(1, 14, 0.0);

  cc.sm.showProbabilityMap();

  vector<int> stepVec;
  stepVec.push_back(1);
  stepVec.push_back(2);
  stepVec.push_back(3);
  stepVec.push_back(4);
  stepVec.push_back(5);
  cs.sm.setAliasVector(stepVec);
  cs.sm.setProbability(1, 2, 1.0);
  cs.sm.setProbability(2, 3, 0.8);
  cs.sm.setProbability(3, 4, 0.6);
  cs.sm.setProbability(4, 5, 1.0);
  cs.sm.showProbabilityMap();

#endif

#ifdef JISSEN
  //to use jissen
  cc.init("data", 1, 4);
  cs.init("step", 1, 2);
  vector<float> ccThre, csThre;
  //set cc threshold
  ccThre.push_back(0.05);
  ccThre.push_back(0.3);
  ccThre.push_back(0.05);
  cc.setThreVec(ccThre);
  //set cs threshold
  csThre.push_back(0.21);
  csThre.push_back(0.35);
  csThre.push_back(0.28);
  cs.setThreVec(csThre);
  vector<int> stateVec;
  stateVec.push_back(1);
  stateVec.push_back(2);
  stateVec.push_back(3);
  stateVec.push_back(4);
  cc.sm.setAliasVector(stateVec);
  cc.sm.setProbabilityEach(1, 2, 1.0);
  cc.sm.setProbabilityEach(2, 3, 1.0);
  cc.sm.setProbabilityEach(1, 3, 1.0);
  cc.sm.setProbabilityEach(1, 4, 1.0);
  cc.sm.setProbabilityEach(2, 4, 1.0);
  cc.sm.setProbabilityEach(3, 4, 1.0);
  cc.sm.showProbabilityMap();

  vector<int> stepVec;
  stepVec.push_back(1);
  stepVec.push_back(2);
  cs.sm.setAliasVector(stepVec);
  cs.sm.setProbabilityEach(1, 2, 1.0);
  cs.sm.showProbabilityMap();

#endif


  cc.setState(1);
  cs.setState(1);



  //cs.setThreshold(0.25, 0.41, 0.30);old_pram
  //cs.setThreshold(0.2, 0.21, 0.20);

  // cout << "convert start" << endl;
  // cc.convertOldFile2NewFile();
  // cs.convertOldFile2NewFile();
  // cout << "convert end" << endl;
  // exit(0);





  // cc.sm.stateChenge(2);
  // cc.sm.stateChenge(3);
  // cout << cc.sm.getCurrentState() << endl;



  firstProc(dirname, image_count, src_img);
  //resizeSrcImg(src_img,src_img, 0.8, INTER_LINEAR);
  resizeSrcImg(src_img,src_img, 0.7, INTER_LINEAR);
  //KLT klt;
  FB fb;
  int feature_num = 200;

  //klt.firstProc(feature_num, src_img);
  fb.firstProc(src_img);

  vector<Point2f> flowpoint, flowscale;
  vector<Point2f> boxedflowpoint, boxedflowscale;
  vector< pair<P2d,P2d> > vals;
  vector< pair<P2d,P2d> > boxedvals;

  float max = 15.0;
  float min = 2.0;
  while (1)
    {

      clock_t start, end;

      //klt.Proc(dirname, image_count, src_img, flowpoint, flowscale);
      fb.Proc(dirname, image_count, src_img, flowpoint, flowscale);

      float max = 15.0;
      float min = 2.0;
      vector<Point2f> largeflowpoint, largeflowscale;

      extractRangeFlow(flowpoint, flowscale, &largeflowpoint, &largeflowscale,  max, min);

      for( uint i = 0 ; i < flowscale.size() ; i++)
      	{
      	  flowscale[i].x *=3;
      	  flowscale[i].y *=3;
       	}

      for( uint i = 0 ; i < largeflowscale.size() ; i++)
      	{
      	  largeflowscale[i].x *=3;
      	  largeflowscale[i].y *=3;
      	}
      //convertflowPoint2P2d(flowpoint, flowscale, &vals);

      convertflowPoint2P2d(largeflowpoint, largeflowscale, &vals);
      vector<Point2f> largeflowpoint2, largeflowscale2;
      extractRangeFlow(flowpoint, flowscale, &largeflowpoint2, &largeflowscale2, 8, 0.0001);
      cv::Mat flow_image;

      display(16, src_img, flow_image, vals, "flow_image");

      /////////////////////////////////////////////////////////////////
      // char savefilename[128];
      // sprintf(savefilename, "save/src_image/frame%04d.ppm",save_count);
      // imwrite(savefilename, src_img);
      // sprintf(savefilename, "save/flow_image/frame%04d.ppm",save_count);
      // imwrite(savefilename, flow_image);

      // cv::Mat hist_image;
      // hist_image = genFlowHist( largeflowscale, largeflowpoint, src_img);
      // sprintf(savefilename, "save/hist_image/frame%04d.ppm",save_count);
      // imwrite(savefilename, hist_image);
      // save_count++;
      /////////////////////////////////////////////////////////////////



      cv::Mat mask;
      calcDnsArea(largeflowpoint2, largeflowscale2, 5, src_img, 4, mask);
      imshow("dnsarea" ,mask);
      imshow("src_image" ,src_img);
      // boxedFlow(flowpoint, flowscale, &boxedflowpoint, &boxedflowscale, 5, src_img);
      // float maxscale = 0.0;
      // calcMaxScale(flowscale, maxscale);
      // convertflowPoint2P2d(boxedflowpoint, boxedflowscale, &boxedvals);
      // display(16, src_img, boxedvals, "boxed");
      char c = waitKey(10);

      if(c == '\x1b')
	{
	  break;
	}
      //else if(c == 's')
      if(image_count >1 && (image_count %2) == 0)
      	{
	  cout << "image_count " << image_count << endl;
	  if(MODE == 0)
	    {
	      // plotFlow(image_count, "P2d", largeflowpoint2, largeflowscale2, 1);
	      // cc.genfiles(image_count, largeflowpoint2, largeflowscale2, src_img);
	      // cs.genfiles(image_count, largeflowpoint2, largeflowscale2, src_img);
	      cc.setState(12);
	      cc.genVecFiles(largeflowpoint2, largeflowscale2, src_img);
	      //cs.genVecFiles(largeflowpoint2, largeflowscale2, src_img);
	    }

      	}
      if(MODE == 1)
	{
	  start = clock();
	  // cc.stateMatch(image_count, largeflowpoint2, largeflowscale2, src_img);
	  // cs.stateMatch(image_count, largeflowpoint2, largeflowscale2, src_img);


	  // cc.stateVecMatch(image_count, largeflowpoint2, largeflowscale2, src_img);
	  // cs.stateVecMatch(image_count, largeflowpoint2, largeflowscale2, src_img);
	  cv::Mat output;
	  cc.stateVecMatch(save_count, largeflowpoint2, largeflowscale2, src_img, output);
	  cs.stateVecMatch(save_count, largeflowpoint2, largeflowscale2, src_img, output);
	  save_count++;

	  end = clock();
	  //printf("%.2f秒かかりました\n",(double)(end-start)/CLOCKS_PER_SEC);
	}
      if(c == 'w')
	{
	  waitKey(0);
	}

      if(image_count == end_count)
	break;

      image_count += 1;



      // cout << "cc.getCheckStateResult " << cc.getCheckStateResult() << endl;
      // cout << "cs.getCheckStateResult " << cs.getCheckStateResult() << endl;
      if(cc.getCheckStateResult() != cs.getCheckStateResult() && cs.getCheckStateResult() != -1 && cc.getCheckStateResult() != -1)
      	{

      	  int currentState_main = cs.getCheckStateResult();
      	  cc.setState(currentState_main);
      	  cs.checkStateResult = -1;


      	}

    }
  return 0;
}
